/***************************************************************************************************************************
* path_planning_utils.h
*
* Author: SFL
*
* Update Time: 2021.4.16
*
* Introduction:  路径规划需要的工具函数

***************************************************************************************************************************/

#ifndef PATH_PLANNING_UTILS_H
#define PATH_PLANNING_UTILS_H


#include <iostream>
#include "CubicSplinePlanner.h"
#include "RangeImpl.h"
#include "wrzf_pkg/DroneState.h"

using namespace CubicSplinePlanner;

Spline2D
generate_target_course(vector<double> x, vector<double> y, vector<double> &rx, vector<double> &ry, vector<double> &ryaw,
                       vector<double> &rc) {
    Spline2D csp(x, y);
    vector<double> ixy;
    // Cosmos::RangeImpl<int> s = Cosmos::Range(0, int(csp.s.back()), 5);
    for (auto i_s : Cosmos::Range(0, int(csp.s.back()), 0.1)) {
        ixy = csp.calc_position(i_s);
        rx.push_back(ixy[0]);
        ry.push_back(ixy[1]);
        ryaw.push_back(csp.calc_yaw(i_s));
        rc.push_back(csp.calc_curvature(i_s));
    }
    return csp;
}

int calc_target_index(wrzf_pkg::DroneState drone_state,
                     vector<double> tx, vector<double> ty,
                     double ahead_distance,
                     double k)
{
    int ind;
    vector<double> dx;
    vector<double> dy;
    vector<double> d;
    for(auto& itx : tx)
    {
        dx.push_back(drone_state.position[0] - itx);
    }
    for(auto& ity : ty)
    {
        dy.push_back(drone_state.position[1] - ity);
    }
    for(int i = 0; i < tx.size(); i++)
    {
        d.push_back(abs(sqrt(dx[i] * dx[i] + dy[i] * dy[i])));
    }
    auto d_min = min_element(d.begin(), d.end());
    ind = distance(d.begin(), d_min);

    double L = 0.0;
    double LF = k * drone_state.velocity_body[0] + ahead_distance;
    while (LF > L && ind + 1 < tx.size())
    {
        L += sqrt(pow(tx[ind + 1] - tx[ind], 2) + pow(ty[ind + 1] - ty[ind], 2));
        ind += 1;
    }

    return ind;
}


geometry_msgs::Point waypoint_trajectory(wrzf_pkg::DroneState _DroneState, geometry_msgs::Point target_position)
{
    geometry_msgs::Point smooth_point;

    if (abs(target_position.x - _DroneState.position[0]) > 3.0)
    {
        smooth_point.x = _DroneState.position[0] + sign_function(target_position.x - _DroneState.position[0]) * 3.0;
    }
    else if (abs(target_position.x - _DroneState.position[0]) < 1.0)
    {
        smooth_point.x = target_position.x;
    }
    else
    {
        smooth_point.x = _DroneState.position[0] + (target_position.x - _DroneState.position[0]) * 0.5;
    }

    if (abs(target_position.y - _DroneState.position[1]) > 3.0)
    {
        smooth_point.y = _DroneState.position[1] + sign_function(target_position.y - _DroneState.position[1]) * 3.0;
    }
    else if (abs(target_position.y - _DroneState.position[1]) < 1.0)
    {
        smooth_point.y = target_position.y;
    }
    else
    {
        smooth_point.y = _DroneState.position[1] + (target_position.y - _DroneState.position[1]) * 0.5;
    }

    if (abs(target_position.z - _DroneState.position[2]) > 3.0)
    {
        smooth_point.z = _DroneState.position[2] + sign_function(target_position.z - _DroneState.position[2]) * 3.0;
    }
    else if (abs(target_position.z - _DroneState.position[2]) < 1.0)
    {
        smooth_point.z = target_position.z;
    }
    else
    {
        smooth_point.z = _DroneState.position[2] + (target_position.z - _DroneState.position[2]) * 0.5;
    }
    

    return smooth_point;
}












#endif //PATH_PLANNING_UTILS_H

